#include "tinyos.h"
#include "tlib.h"


task_t *cur_task;
task_t *next_task;
static u8 sched_lock_cnt;
static struct list_head task_table[TINYOS_PRO_COUNT];
static bitmap_t task_prio_table;

void task_sched_init(void)
{
    int i;
    
    sched_lock_cnt = 0;
    bitmap_init(&task_prio_table);
    for (i = 0; i < ARRAY_SIZE(task_table); i++) {
        INIT_LIST_HEAD(&task_table[i]);
    }
}

void task_sched_ready(task_t *task)
{
    bitmap_set(&task_prio_table, task->prio);
    task->state |= TINYOS_TASK_STATE_READY;
    list_del_init(&task->entry);
    list_add_tail(&task->entry, &task_table[task->prio]);
}

void task_sched_unready(task_t *task)
{
    list_del_init(&task->entry);
    task->state &= ~TINYOS_TASK_STATE_READY;
    if (list_empty(&task_table[task->prio])) {
        bitmap_clear(&task_prio_table, task->prio);
    }
}

void task_sched_enable(void)
{
    u32 status;
    
    status = task_enter_critical();
    if (sched_lock_cnt > 0) {
        if (--sched_lock_cnt == 0) {
            task_schedule();
        }
    }
    task_exit_critical(status);
}

void task_sched_disable(void)
{
    u32 status;
    
    status = task_enter_critical();
    if (sched_lock_cnt < 255) {
        sched_lock_cnt++;
    }
    task_exit_critical(status);
}

static void task_stack_init(task_t *task)
{
    task_stack_t *stack;
    
    stack = task->stack;
    *(--stack) = (unsigned long)(1 << 24);
    *(--stack) = (unsigned long)task->func;
    *(--stack) = (unsigned long)0x14;
    *(--stack) = (unsigned long)0x12;
    *(--stack) = (unsigned long)0x3;
    *(--stack) = (unsigned long)0x2;
    *(--stack) = (unsigned long)0x1;
    *(--stack) = (unsigned long)task->param;
    
    *(--stack) = (unsigned long)0x11;
    *(--stack) = (unsigned long)0x10;
    *(--stack) = (unsigned long)0x9;
    *(--stack) = (unsigned long)0x8;
    *(--stack) = (unsigned long)0x7;
    *(--stack) = (unsigned long)0x6;
    *(--stack) = (unsigned long)0x5;
    *(--stack) = (unsigned long)0x4;
    task->stack = stack;
}

task_t *task_get_ready(void)
{
    u32 highest_prio;
    
    highest_prio = bitmap_get_first(&task_prio_table);
    
    return highest_prio < TINYOS_PRO_COUNT ? 
        list_first_entry(&task_table[highest_prio], task_t, entry) : NULL;
}

void task_schedule(void)
{
    u32 status;
    task_t *task;
    u32 need_schedule;
    
    need_schedule = 0;
    status = task_enter_critical();
    if (sched_lock_cnt > 0) {
        task_exit_critical(status);
        return;
    }
    task = task_get_ready();
    if (task && (task != cur_task)) {
        next_task = task;
        need_schedule = 1;
    }
    task_exit_critical(status);
    if (need_schedule) {
        os_schedule();
    }
}

void task_wakeup(task_t *task)
{
    task_sched_ready(task);
}


void task_init(task_t *task, task_handle_t handle, void *param, 
    task_stack_t *stack, u32 len, u32 prio)
{
    task->stack = stack;
    task->stack_size = len;
    task->func = handle;
    task->param = param;
    task->prio = prio;
    task->delay_tick = 0;
    task->suspend_cnt = 0;
    task->slice = TINYOS_SLICE_MAX;
    task_stack_init(task);
    INIT_LIST_HEAD(&task->entry);
    task_sched_ready(task);
}

void task_supend(task_t *task)
{
    u32 status;
    status = task_enter_critical();
    if (!(task->state & TINYOS_TASK_STATE_DELAYED)) {
        if (task->suspend_cnt++ == 0) {
            task->state |= TINYOS_TASK_STATE_PENDED;
            task_sched_unready(task);
            if (task == cur_task) {
                task_schedule();
            }
        }
    }
    
    task_exit_critical(status);
}

void task_resume(task_t *task)
{
    u32 status;
    status = task_enter_critical();
    if ((task->state & TINYOS_TASK_STATE_PENDED)) {
        if (--task->suspend_cnt == 0) {
            task->state &= ~TINYOS_TASK_STATE_PENDED;
            task_sched_ready(task);
            task_schedule();
        }
    }
    
    task_exit_critical(status);    
}